/******************************************************************************
* AIR SYSTEMS LABORATORY
* AEROSPACE ENGINEERING
* UNIVERISTY OF TEXAS AT AUSTIN
* UAV FLIGHT SOFTWARE 
* DESCRIPTION:
*   
*
* SOURCE: 
* Orignial: 08/14/2008  Jorge Alvarez joroge@gmail.com
* Modified: 03/20/2009	Sergio García-Nieto sergarro@isa.upv.es
* $Id: adc.c 4 2009-07-21 17:12:11Z joroge@gmail.com $
*************************************************************************************/

#include <stdlib.h>
#include <stdio.h>
#include <stdio.h>
#include <string.h>
#include <unistd.h>
#include <fcntl.h>
#include <errno.h>
#include <termios.h>
#include <math.h>
#include <pthread.h>
#include <time.h>

#include "adc.h"
#include "serial.h"
#include "globaldefs.h"
#include "utilities.h"

void adcInitialization()
{
	int 	    nbytes=0;
	char        reset2Polling[4]={'~','m','0','\n'}; //reset to polling
	char        resetDiffPress[3]={'~','v','\n'}; //reset negative values for velocity
	char        resetEprom[3]={'~','e','\n'}; //reset negative values for velocity
	char        resetSeaPressure[8]={'~','r','1','0','1','3','3','\n'}; //reset sea level pressure
	char		setAltitude[6]={'~','h','1','8','9','\n'}; //set current altitude in [m]
	char		setBinaryProtocol[3]={'~','b','\n'}; //changue from ASCII to binary protocol
	char		setAsciiProtocol[3]={'~','a','\n'}; //changue from ASCII to binary protocol
	int 	  	sPort;
	
			 
	 /*********************************************************************
	 *Opening and configuring ADC
	 *********************************************************************/
	sPort = open_serial(SERIAL_PORT4,BAUDRATE_38400); 
 	 nbytes = write(sPort,(char*)&setAsciiProtocol,3);
	 //printf("%d\n",nbytes);
	 nbytes = 0;
  	 nbytes = write(sPort,(char*)&resetSeaPressure,8);
	 //printf("%d\n",nbytes);
	 nbytes = 0;
	 nbytes = write(sPort,(char*)&resetDiffPress,3);     
	 //printf("%d\n",nbytes);
	 nbytes = 0;
	 nbytes = write(sPort,(char*)&setAltitude,6);
	 //printf("%d\n",nbytes);
	 nbytes = 0;
	 nbytes = write(sPort,(char*)&reset2Polling, 4);     
	//printf("%d\n",nbytes);
	nbytes = 0;
  	nbytes = write(sPort,(char*)&setBinaryProtocol,3);
	 //printf("%d\n",nbytes);
	 nbytes = 0;
 	 nbytes = write(sPort,(char*)&resetEprom,3);
	 //printf("%d\n",nbytes);
	 close(sPort);
	 
}	 

void adcRequest(int sPort)
{
	int 	    nbytes=0;
	byte        ADCpacketRequest[7]={SYNC0,SYNC1,ADC_PACKET_ID,0x00,CS0,CS1};
	/*********************************************************************
	 *Send ADC Command to ADC
	*********************************************************************/
	//nbytes = write(sPort,(char*)&reset2Polling, 4);  
	nbytes = write(sPort,(char*)&ADCpacketRequest, 7);
	//printf("Write ADC Function %d\n",nbytes);	
}	 
void adcAcquisition(int sPort,struct adc *adcPacket)
{	
	int 	    nbytes=0;
	byte  		input_ADC[ADC_PACKET_LENGTH]={0,};

	
	/*********************************************************************
	 *Read ADC
	*********************************************************************/
	//printf("Despierto %d\n",nbytes);
	nbytes=read(sPort,input_ADC,ADC_PACKET_LENGTH);
	//printf("Read ADC Function %d\n",nbytes);
	//printf("[ADC]:cadena %s ...!\n",input_ADC);
	/*********************************************************************
	 *Read packet contents
	*********************************************************************/
	if(nbytes>0)
	{	
	if(checksumCheckADC(input_ADC,ADC_PAYLOAD)==TRUE)
	 	{
		//printf("[ADC]:Nice,it worked ...!\n");
		/* delta velocity's in m/s */ 
		//printf("[ADC]:cadena %s ...!\n",input_ADC);
		adcPacket->pAltitude=(float)(sIntFromBytesBE(&input_ADC[12])/10.0); 
		adcPacket->airSpeed=(float)(uSintFromBytesBE(&input_ADC[24])/10.0); 
	  	}
	else 
		{
		//printf("[ADC]:Checksum error..!\n");
		}
	}
}

int checksumCheckADC(byte* buffer, int packet_len)
{
   short     	 i=0;
   byte			 ck_a=0,ck_b=0;
   byte			 ck_a_p=0,ck_b_p=0;
 
   for(i=2;i<packet_len-2;i++)
    {
   		ck_a=ck_a + buffer[i];
   		ck_b=ck_b + ck_a;
   		}
   ck_a_p = buffer[packet_len-2];
   ck_b_p = buffer[packet_len-1];
   if ((ck_a_p==ck_a)& (ck_b_p==ck_b))
   	return    TRUE;
   else
 	return    FALSE;
}


